/**********************************************************************
 *                                                                    *
 * tgt - Tiny Graphics Toolbox                                        *
 *                                                                    *
 * Copyright (C) 2006-2008 Visualization and Computer Graphics Group, *
 * Department of Computer Science, University of Muenster, Germany.   *
 * <http://viscg.uni-muenster.de>                                     *
 *                                                                    *
 * This file is part of the tgt library. This library is free         *
 * software; you can redistribute it and/or modify it under the terms *
 * of the GNU Lesser General Public License version 2.1 as published  *
 * by the Free Software Foundation.                                   *
 *                                                                    *
 * This library is distributed in the hope that it will be useful,    *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of     *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the       *
 * GNU Lesser General Public License for more details.                *
 *                                                                    *
 * You should have received a copy of the GNU Lesser General Public   *
 * License in the file "LICENSE.txt" along with this library.         *
 * If not, see <http://www.gnu.org/licenses/>.                        *
 *                                                                    *
 **********************************************************************/

#include <stdlib.h>
#include "tgt/navigation/walknavigation.h"

#include <typeinfo>

using namespace tgt;


WalkNavigation::WalkNavigation(
        GLCanvas* GLCanvas,
        Camera* camera,
        int button1,
        int button2,
        const Vector& onGroundPlane,
        double minHeight,
        double refHeight,
        double walkSpeed,
        double runSpeed,
        double verticalTime,
        double horizontalAccelerationTime,
        double verticalAccelerationTime
        )
    : Navigation ( GLCanvas )
{
    EPS = 1e-5;
    MaxAltitude = 89.9;
    canvas_ = GLCanvas;
    camera_ = camera;

    button1_ = button1;
    button2_ = button2;

    // configuration variables (cannot changed via mouse or keyboard)
    walkSpeed_      = walkSpeed;
    runSpeed_       = runSpeed;
    walkSpeed_      = 2;
    runSpeed_       = 3;
    verticalTime_   = verticalTime;

    refHeight_ = 0.17;

    minHeight_      = minHeight;
    refHeight_      = refHeight;

    horizontalAccelerationTime_ = horizontalAccelerationTime;
    verticalAccelerationTime_   = verticalAccelerationTime;

    sensitivity_ = 3;

    horizontalScaling_  = 1.;
    verticalScaling_    = 1.;

    onGroundPlane_ = onGroundPlane;

    keyTimeout_ = 0;

    enableMouseButtonAcceleration( MouseEvent::MOUSE_BUTTON_3 );

    // control keys
    frontKey_ = KeyEvent::K_UP;
    backKey_  = KeyEvent::K_DOWN;
    leftKey_  = KeyEvent::K_LEFT;
    rightKey_ = KeyEvent::K_RIGHT;
    upKey_    = KeyEvent::K_PAGEUP;
    downKey_  = KeyEvent::K_PAGEDOWN;
    runKey_   = ' ';
    turnAroundKey_ = 'T';
    enableKeys_ = true;


    // state variables
    running_        = false;

    forwardPressed_ = false;
    backwardPressed_ = false;
    leftPressed_ = false;
    rightPressed_ = false;
    upPressed_ = false;
    downPressed_ = false;
    turnKeyPressed_ = false;

    currentForwardSpeed_ = 0;
    currentSidewardSpeed_ = 0;
    verticalSpeedAmount_ = 0;

    accelerationButtonPressed_ = false;
    turnButtonPressed_ = false;
    moveButtonPressed_ = false;

    // key timeouts
    forwardTimeout_     = 0;
    sidewardTimeout_    = 0;
    upDownTimeout_      = 0;

    // other
    lastX_          = 0; // lastX_/lastY_ will be initialized correctly in the dragStart method
    lastY_          = 0;
    dx_             = 0; // When the mouse button is hold down, the summed x-/y-rotation is stored in
    dy_             = 0; // dx_ and dy_
    turnAroundSteps_ = 0; // re-initialized in the onKey method, if a turn-around is involved
    elapsedTime_    = 0; // elapsedTime_ is increased with each timeevent. If it reaches 1 tick
                         // (1/50 seconds), one navigation step is performed and elapsedTime_
                         // is decreased by one tick.

    s_constr_ = false;
}


//WalkNavigation::~WalkNavigation() {}

void WalkNavigation::enableMouseButtonAcceleration( int button ) {

	mouseButtonAccelerationButton_ = button;
	enableMouseButtonAcceleration_ = true;
}

void WalkNavigation::disableMouseButtonAcceleration() {
	enableMouseButtonAcceleration_ = false;
}

void WalkNavigation::onEvent(Event* event) {
	if ( typeid( *event ) == typeid( MouseEvent ) ) {
		MouseEvent* e = ( MouseEvent* ) event;

		if ( e->action() == MouseEvent::PRESSED ) {
			int x = e->x();
			int y = e->y();

			if( e->button() == button1_ ) {
                lastX_ = x;
                lastY_ = y;

                turnButtonPressed_ = true;
			}

			if( e->button() == button2_ ) {
                lastX_ = x;
                lastY_ = y;

                moveButtonPressed_ = true;
			}

			if ( e->button() == mouseButtonAccelerationButton_ )  {
				accelerationButtonPressed_ = true;
			}

			e->ignore();

// FIXME: setNavigationStatus(..)
//			setNavigationStatus( Navigation::NAVIGATION_WALK_ACTIVE );
		}

		if( e->action() == MouseEvent::MOTION ) {
			double h = camera_->getPosition().y * 0.025;

			from_ = camera_->getPosition();
			to_ = camera_->getFocus();
			y_up_ = Vector( 0.0f, 1.0f, 0.0f );
			dx_ = 0;
			dy_ = 0;

			if ( turnButtonPressed_ && !moveButtonPressed_ ){
                double dx = - (double) ( e->x() - lastX_ ) * sensitivity_ * 0.1;
				double dy =   (double) ( e->y() - lastY_ ) * sensitivity_ * 0.1;
				lastX_ = e->x();
				lastY_ = e->y();
				dx_+= dx;
				dy_+= dy;
				vec3 look = quat::rotate( to_ - from_, 0.01f * (float) dx_, y_up_);
				camera_->setFocus( from_ + normalize(look) - y_up_ * (float) dy_ * 0.01f );
			}
			// mouse movement and zoomButton pressed
			if ( !turnButtonPressed_ && moveButtonPressed_ ){
				double dx = (double) ( e->x() - lastX_ ) * 0.1;
				double dy = (double) ( e->y() - lastY_ ) * 0.1;
				lastX_ = e->x();
				lastY_ = e->y();
				dx_+= dx;
				dy_+= dy;
				// check position
                if (true) {
                    //TODO: define focusPoint by mouseclick
                    vec3 focusPoint = to_;
                    vec3 look = quat::rotate( to_ - from_, 0.05f * (float) dx_, y_up_);
                    vec3 dir = length( camera_->getFocus() - camera_->getPosition() ) * normalize(look);
                    camera_->setPosition( ( focusPoint - dir ) - (float) h * 0.5f * (float) dy_* normalize(y_up_) );
                    camera_->setFocus( focusPoint - (float) h * 0.5f *(float) dy_ * normalize(y_up_) );
				}
			}
			e->ignore();
		}


		if( e->action() == MouseEvent::RELEASED ) {
			if( e->button() == mouseButtonAccelerationButton_ ){
				accelerationButtonPressed_ = false;
			}
			if( e->button() == button1_ ){
				turnButtonPressed_ = false;
			}
			if( e->button() == button2_ ){
				moveButtonPressed_ = false;
			}

//FIXME: setNavigationStatus
//			setNavigationStatus(Navigation::NAVIGATION_WALK_INACTIVE);
		}
	}


	if( typeid( *event ) == typeid( KeyEvent ) ) {
		KeyEvent* e = ( KeyEvent* ) event;
		unsigned int key = e->keyCode();
		if( enableKeys_ ){
			if( !e->pressed() ){
			  if ( key == frontKey_ || key == backKey_ ) {
					forwardPressed_ = false;
					backwardPressed_ = false;
			  }
			  if ( key == leftKey_ || key == rightKey_ ) {
					leftPressed_ = false;
					rightPressed_ = false;
			  }
			  if ( key == upKey_ || key == downKey_) {
					upPressed_ = false;
					downPressed_ = false;
			  }
			  if ( key == turnAroundKey_ ) {
					turnKeyPressed_ = false;
			  }

//FIXME: setNavigationStatur
//			  setNavigationStatus(Navigation::NAVIGATION_WALK_ACTIVE);
			}

			if(e->pressed()){
				if ( key == frontKey_ ) {
					forwardPressed_ = true;
					backwardPressed_ = false;
					forwardTimeout_ = keyTimeout_;
				}
				else if ( key == backKey_ ) {
					forwardPressed_ = false;
					backwardPressed_ = true;
					forwardTimeout_ = keyTimeout_;
				}
				else if ( key == leftKey_ ) {
					leftPressed_ = true;
					rightPressed_ = false;
					sidewardTimeout_ = keyTimeout_;
				}
				else if ( key == rightKey_ ) {
					leftPressed_ = false;
					rightPressed_ = true;
					sidewardTimeout_ = keyTimeout_;
				}
				else if ( key == runKey_ ) {
					running_ = true;
				}
				else if ( key == upKey_ ) {
					upPressed_ = true;
					downPressed_ = false;
					upDownTimeout_ = keyTimeout_;
				}
				else if ( key == downKey_ ) {
					downPressed_ = true;
					upPressed_ = false;
					upDownTimeout_ = keyTimeout_;
				}
				else if ( key == turnAroundKey_ ) {
					turnKeyPressed_=true;
				}
			}

			e->ignore();

//FIXME: setNavigationStatus
//			setNavigationStatus(Navigation::NAVIGATION_WALK_INACTIVE);
		}
	}

	if( typeid( *event ) == typeid( TimeEvent ) ) {
		vec3 up = normalize( camera_->getUpVector() );
        vec3 newFrom = camera_->getPosition();
        double height = camera_->getPosition()[1];


        // calculate acceleration: 'walkSpeed_' shall be reached in 'horizontalAccelerationTime_'
        // seconds, i.e. in 50*horizontalAccelerationTime_ cycles.
        double horizontalAcceleration = walkSpeed_ / (50. * horizontalAccelerationTime_);

        // currentForwardSpeed_ contains the current speed without considering 'height' and
        // possible horizontalScaling
        if ( forwardPressed_ || accelerationButtonPressed_ ) {
          currentForwardSpeed_ += horizontalAcceleration;
        }
        else {
          // slow down
          if ( currentForwardSpeed_ > 0 ){
                currentForwardSpeed_ -= horizontalAcceleration;
                if ( currentForwardSpeed_ < 0 ) {
                     currentForwardSpeed_ = 0;
                }
          }
        }

        if ( backwardPressed_ ) {
          currentForwardSpeed_ -= horizontalAcceleration;
        }
        else {
          // slow down
          if ( currentForwardSpeed_ < 0 ) {
                currentForwardSpeed_ += horizontalAcceleration;
                if ( currentForwardSpeed_ > 0 ) {
                     currentForwardSpeed_ = 0;
                }
          }
        }

        // currentSidwardSpeed_ contains the current speed without considering 'height' and
        // possible horizontalScaling
        if (rightPressed_) {
          currentSidewardSpeed_ += horizontalAcceleration;
        }
        else {
          // slow down
          if (currentSidewardSpeed_>0) {
                currentSidewardSpeed_ -= horizontalAcceleration;
                if (currentSidewardSpeed_<0) {
                     currentSidewardSpeed_ = 0;
                }
          }
        }

        if (leftPressed_) {
          currentSidewardSpeed_ -= horizontalAcceleration;
        }
        else {
          // slow down
          if (currentSidewardSpeed_<0) {
                currentSidewardSpeed_ += horizontalAcceleration;
                if (currentSidewardSpeed_>0) {
                     currentSidewardSpeed_ = 0;
                }
          }
        }

        double maxSpeed = (running_ ? runSpeed_ : walkSpeed_);
        currentForwardSpeed_ = cropValue(currentForwardSpeed_, -maxSpeed, maxSpeed);
        currentSidewardSpeed_ = cropValue(currentSidewardSpeed_, -maxSpeed, maxSpeed);

        // scale the height values
        double scaledMinHeight = minHeight_ * verticalScaling_;
        double scaledRefHeight = refHeight_ * verticalScaling_;

        // The movement speed depends on the distance 'height' of the camera from the ground plane.
        // Calculate the factor introduced by the height.
        // Below minHeight the speed keeps constant, but height can only be smaller than minHeight, if
        // the lookat has been changed externally
        double heightFactor = (height < scaledMinHeight ? (scaledMinHeight/scaledRefHeight) : (height/scaledRefHeight));
        heightFactor = ::sqrt(heightFactor);

        double scaledForwardSpeed = heightFactor * currentForwardSpeed_ * horizontalScaling_ / 50.;
        double scaledSidewardSpeed = heightFactor * currentSidewardSpeed_ * horizontalScaling_ / 50.;


        // move forward/backward
        if ( scaledForwardSpeed != 0 ) {
            vec3 lookDir = camera_->getFocus()-camera_->getPosition();
            vec3 walkDir = normalize( makeOrthogonal(lookDir, up) );
            walkDir.y = 0.f;
            walkDir *= (float)scaledForwardSpeed;

            newFrom = newFrom + walkDir;
        }

        if ( scaledSidewardSpeed != 0 ) {
            vec3 lookDir = camera_->getFocus()-camera_->getPosition();
            vec3 walkDir = normalize( cross(lookDir,up) );
            walkDir *= (float)scaledSidewardSpeed;

            newFrom = newFrom+walkDir;
        }


        double speedInc = 1. / ( 25. * verticalAccelerationTime_ );

        if ( upPressed_ ) {
            verticalSpeedAmount_ += speedInc;
        }
        else { // slow down
            if ( verticalSpeedAmount_ > 0 ) {
                verticalSpeedAmount_ -= speedInc;
                if ( verticalSpeedAmount_ < 0 ) {
                    verticalSpeedAmount_ = 0;
                }
            }
        }

        if (downPressed_) {
            verticalSpeedAmount_ -= speedInc;
        }
        else { // slow down
            if ( verticalSpeedAmount_ < 0 ) {
                verticalSpeedAmount_ += speedInc;
                if ( verticalSpeedAmount_ > 0 ) {
                     verticalSpeedAmount_ = 0;
                }
            }
        }

        verticalSpeedAmount_ = cropValue( verticalSpeedAmount_, -1., 1. );


        // should be true normally. Can only be false if lookat is changed externally
        bool aboveGround = ( dot( camera_->getPosition()-onGroundPlane_, up ) >= 0 );

        /*  When the camera moves up and down, each time cycle 'height' is multiplied with a positive
            factor f>1 (up) or f<1 (down). Since 'height' should be doubled (or halved) in
            'verticalTime' seconds, f is given by f^(50*verticalTime) = 2 (or f^(50*verticalTime) = 1/2). */
        static const double exp = 2.718281828459;
        double f = pow( exp, log(2.) / ( 50. * verticalTime_ ) );
        // (Since this part is called only 50 times per second, there is no need for performance optimization)

        // To control smooth acceleration by 'verticalSpeedAmount', we exponentiate 'f' with it.
        f = pow( f, verticalSpeedAmount_ );

        // move up/down
        if ( verticalSpeedAmount_ != 0 ) {

            double heightChange = 0;

            if ( height >= scaledMinHeight ) {
                // Regular case:
                double newHeight = height * f;
                heightChange = newHeight - height;
            }
            else {
                // The camera is below the allowed minimum height.
                // This can only happen if the lookat is changed externally.
                // To prevent freezing we allow only upward movement
                // and perform this with constant speed. This constant speed
                // is the vertical speed which would have been chosen for
                // height = minHeight
                heightChange = scaledMinHeight * (f-1);
            }

            if ( aboveGround && height >= scaledMinHeight && height + heightChange < scaledMinHeight ) {
                // avoid leaving allowed area
                heightChange = scaledMinHeight - height;
            }

            if ( height < scaledMinHeight ) {
                // if the camera is already below the allowed minimum height,
                // only allow upward movement
                if ( heightChange < 0 ) heightChange = 0;
            }

            if ( heightChange != 0 ) {

            newFrom = newFrom + up * (float) heightChange;
            }
        }

        vec3 dir = camera_->getFocus() - camera_->getPosition();

// FIXME: checkPosition
		#ifndef _3DSP_NOCOLLISION
//			bool checkPos = checkPosition(newFrom);
//				// ohne Kollision
//			if (checkPos == true) {
//				camera_->setPosition(newFrom);
//				camera_->setFocus(newFrom + dir);
//			}
		#else // _3DSP_NOCOLLISION
			camera_->setPosition(newFrom);
			camera_->setFocus(newFrom + dir);
		#endif

		canvas__->update();
	}

	event->ignore();
}

//Fixme: Move to .h for inlining
void WalkNavigation::setSensitivity( double s ) { sensitivity_ = s; }
void WalkNavigation::setGroundPlaneVector( const vec3& v ) { onGroundPlane_ = v; }
void WalkNavigation::setMinHeight( double h ) { minHeight_ = h; }
void WalkNavigation::setReferenceHeight( double h ) { refHeight_ = h; }
void WalkNavigation::setWalkSpeed( double s ) { walkSpeed_ = s; }
void WalkNavigation::setRunSpeed( double s ) { runSpeed_ = s; }
void WalkNavigation::setVerticalTime( double t ) { verticalTime_ = t; }
void WalkNavigation::setHorizontalAccelerationTime( double at ) { horizontalAccelerationTime_ = at; }
void WalkNavigation::setVerticalAccelerationTime( double at ) { verticalAccelerationTime_ = at; }
void WalkNavigation::setKeyTimeout( unsigned int t ) { keyTimeout_ = t; }

void WalkNavigation::setKeys( unsigned int front, unsigned int back, unsigned int left, unsigned int right,
                              unsigned int up, unsigned int down, unsigned int toggleSpeed, unsigned int turnAround ) {
    frontKey_ = front;
    backKey_  = back;
    leftKey_  = left;
    rightKey_ = right;
    upKey_    = up;
    downKey_  = down;
    runKey_   = toggleSpeed;
    turnAroundKey_ = turnAround;
}

void WalkNavigation::enableKeys() { enableKeys_ = true; }

void WalkNavigation::disableKeys() { enableKeys_ = false; }

void WalkNavigation::setScaling( double hor, double vert ) {
    horizontalScaling_ = hor;
    verticalScaling_ = vert;
}
